#include "src/Base_Function/Base_Function.h"
#include "src/Grab/Grab.h"

void setup()
{
  BF_Init();
}

// bool test = true;
// int temp = 0;

void loop()
{
  // 读取传感器电平
  SL_2 = digitalRead(SensorLeft_2);
  SR_2 = digitalRead(SensorRight_2);
  SL_1 = digitalRead(SensorLeft_1);
  SR_1 = digitalRead(SensorRight_1);

  if (SL_1 == HIGH && SR_1 == HIGH) // 若避障传感器同时返回高电平(避障灯都灭)
  {
    if (SL_2 == HIGH && SR_2 == LOW)
      chassis_leftturn(50);

    else if (SL_2 == LOW && SR_2 == HIGH)
      chassis_rightturn(50);

    else if (SL_2 == HIGH && SR_2 == HIGH)
      chassis_accelerate(80);

    else
      chassis_brake();
  }

  else if (SL_1 == HIGH && SR_1 == LOW)
    chassis_rightturn(50);

  else if (SL_1 == LOW && SR_1 == HIGH)
    chassis_leftturn(50);

  if (SL_2 == LOW && SR_2 == LOW) // 若循迹传感器同时返回低电平(循迹灯都亮)
  {
    //      temp++;
    //      if(temp>3000)
    //      {
    //        if(test)
    //        {
    //          rightturn(60,70);
    //          }
    //         else
    //        {
    //            leftturn(60,70);
    //          }
    //          test=!test;
    //          temp=0;
    //        }
    Grab();
  }
}
